#include "Pallocator.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <pthread.h>


static const int slot_size [128] = 
{
    2,       6,      10,      14,
    18,      22,      26,      30,
    34,      38,      42,      46,
    50,      54,      58,      62,
    66,      70,      74,      78,
    82,      86,      90,      94,
    98,     102,     106,     110,
    114,     118,     122,     126,

    134,     142,     150,     158,
    166,     174,     182,     190,
    198,     206,     214,     222,
    230,     238,     246,     254,
    262,     270,     278,     286,
    294,     302,     310,     318,
    326,     334,     342,     350,
    358,     366,     374,     382,

    398,     414,     430,     446,
    462,     478,     494,     510,
    526,     542,     558,     574,
    590,     606,     622,     638,
    654,     670,     686,     702,
    718,     734,     750,     766,
    782,     798,     814,     830,
    846,     862,     878,     894,

    926,     958,     990,    1022,
    1054,    1086,    1118,    1150,
    1182,    1214,    1246,    1278,
    1310,    1342,    1374,    1406,
    1438,    1470,    1502,    1534,
    1566,    1598,    1630,    1662,
    1694,    1726,    1758,    1790,
    1822,    1854,    1886,    1918,
};
/*
int main()
{
    int base = -2;
    for(int i = 0; i < 32; ++i)
    {
        base += 4;
        printf("%8d,", base);
        if (i%4 == 3)
            printf("\n");
    }
    printf("\n");

    for(int i = 0; i < 32; ++i)
    {
        base += 8;
        printf("%8d,", base);
        if (i%4 == 3)
            printf("\n");
    }
    printf("\n");
    for(int i = 0; i < 32; ++i)
    {
        base += 16;
        printf("%8d,", base);
        if (i%4 == 3)
            printf("\n");
    }
    printf("\n");
    for(int i = 0; i < 32; ++i)
    {
        base += 32;
        printf("%8d,", base);
        if (i%4 == 3)
            printf("\n");
    }
}
*/


#define SIZE_SLOT(x)  x<126?((x+1)>>2):(x<382?((x-127)>>3)+32:((x<894?((x-383)>>4)+64:(((x-895)>>5))+96)))
#define SLOT_ITEMS(x) (x<32?256:(x<64?128:(x<96?64:32)))
//#define SLOT_ITEMS(x) (x<32?32:(x<64?32:(x<96?16:8)))
#define SYS_MALLOCED 255

#pragma pack(push)
#pragma pack(1)
struct MemBucket
{
    MemBucket *prev, *next;
    unsigned char bitmap[32];
    unsigned int count;
    unsigned int  magic;
    unsigned char content[0];
};
#pragma pack(pop)


static MemBucket *slots[128 * 2];
static pthread_spinlock_t lock[128];

void painit()
{
    for(int i = 0; i < 128; ++i)
    {
        slots[i * 2] = NULL;
        slots[i * 2 + 1] = NULL;
        pthread_spin_init(&lock[i], 0);
    }

}



void *palloc(int size)
{
    int slot = SIZE_SLOT(size);
    int res;
    MemBucket *cur;
    if (slot < 128)
    {
        int ret_pos;
        int item_num = SLOT_ITEMS(slot);

        pthread_spin_lock(&lock[slot]);
        cur = slots[slot * 2];
        if (cur == NULL)
        {
            int msize = sizeof(MemBucket) + (slot_size[slot] + 2) * item_num;
            cur = (MemBucket*)malloc(msize);
            if (cur == NULL) return NULL;
            cur->prev = cur->next = NULL;
            slots[slot * 2] = cur;
            for(int i = 0; i < (item_num>>3); ++i)
                cur->bitmap[i] = 0;
            cur->count = 0;
            cur->magic = 0x19890604;
        }

        for(int k = 0; k < (item_num >> 5); ++k)
        {
            int i = k * 4;
            if (*(unsigned int*)&(cur->bitmap[i]) == 0xffffffff)
                continue;
            for(; i < (item_num >> 3); ++i) 
            if (cur->bitmap[i] != 0xff)
            {
                unsigned char cb = cur->bitmap[i];
                for(int j = 0; j < 8; ++j)
                {
                    if (((cb >> j)&0x1) == 0)
                    {
                        ret_pos = i * 8 + j;
                        cur->bitmap[i] |= (1 << j);
                        cur->count++;
                        break;
                    }
                }
                break;
            }
            break;
        }
       
        if (cur->count == item_num)
        {
            slots[slot * 2] = cur->next;
            if (cur->next != NULL) cur->next->prev = NULL;
            cur->next = slots[slot * 2 + 1];
            if (cur->next != NULL) cur->next->prev = cur;
            slots[slot * 2 + 1] = cur;
        }
        

        unsigned char *res = &(cur->content[0]) + ret_pos * (slot_size[slot] + 2) ;
        *res++ = slot;
        *res++ = ret_pos;
        pthread_spin_unlock(&lock[slot]);
        return res;
    }
    else
    {
        void *sys_malloced = malloc(size+2);
        if (sys_malloced == NULL) return NULL;
        *(unsigned char *)sys_malloced = SYS_MALLOCED;
        return (char*)sys_malloced + 2;
    }
    return NULL;
}


void pafree(void *mem)
{
    unsigned char *buf = (unsigned char*)mem;
    MemBucket *cur;
    int item_idx, slot;
    buf -= 2;
    if (*buf == SYS_MALLOCED)
    {
        free(buf);
        return;
    }
    slot = *buf;
    item_idx = *(buf + 1);
    int item_num = SLOT_ITEMS(slot);

    pthread_spin_lock(&lock[slot]);
    
    cur = (MemBucket*)(buf - item_idx * (slot_size[slot] + 2) - sizeof(MemBucket));
    if (cur->magic != 0x19890604) abort();
    if (cur->count != item_num)
    {
        cur->bitmap[item_idx>>3] &= (~(1<<(item_idx%8)));
        cur->count--;
        if (!cur->count)
        {
            if (cur->prev) cur->prev->next = cur->next;
            if (cur->next) cur->next->prev = cur->prev;
            if (slots[slot * 2] == cur) slots[slot * 2] = cur->next;
            free(cur);
        }
    }
    else
    {
        if (cur->next != NULL) cur->next->prev = cur->prev;
        if (cur->prev != NULL) cur->prev->next = cur->next;
        if (slots[slot * 2 + 1] == cur) slots[slot * 2 + 1] = cur->next;
        cur->next = slots[slot * 2];
        cur->prev = NULL;
        if (cur->next != NULL) cur->next->prev = cur;
        slots[slot * 2] = cur;
        cur->bitmap[item_idx>>3] &= (~(1<<(item_idx%8)));
        cur->count--;
    }    

    pthread_spin_unlock(&lock[slot]);
    return;
}

